﻿using System;
using System.Diagnostics;
using System.Globalization;
using RimbaseLib;

namespace Controller_Interface
{
    ///<summary>
    /// 
    ///</summary>
    public class ObjectHelper
    {
        ///<summary>
        /// Fulfills the position parameters from the string input 
        ///</summary>
        ///<param name="robTarget"></param>
        ///<param name="input"></param>
        ///<returns></returns>
        public static bool FillRobTargetPosition(S4RobTarget robTarget, String input)
        {
            
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            short positionIndex = 0;

            string[] words = input.Split(new[] { ' ', ',', '[', ']' });

            foreach (string s in words)
            {
                if (s.Length > 0)
                {
                    try
                    {
                        float value = float.Parse(s,nfi);
                        robTarget.set_Pos(positionIndex, value);
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine(e.ToString());
                        return false;
                    }
                    positionIndex++;
                }
            }

            return true;
        }

        ///<summary>
        ///</summary>
        ///<param name="robTarget"></param>
        ///<returns></returns>
        public static String GetRobTargetPosition(S4RobTarget robTarget)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("[{0},{1},{2}]", robTarget.get_Pos(0).ToString(nfi), robTarget.get_Pos(1).ToString(nfi),
                                 robTarget.get_Pos(2).ToString(nfi));
        }

        ///<summary>
        ///</summary>
        ///<param name="robTarget"></param>
        ///<returns></returns>
        public static String GetRobTargetOrientation(S4RobTarget robTarget)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("[{0},{1},{2},{3}]", robTarget.get_Orient(0).ToString(nfi), robTarget.get_Orient(1).ToString(nfi),
                                 robTarget.get_Orient(2).ToString(nfi), robTarget.get_Orient(3).ToString(nfi));
        }

        ///<summary>
        ///</summary>
        ///<param name="robTarget"></param>
        ///<param name="input"></param>
        ///<returns></returns>
        public static bool FillRobTargetOrientation(S4RobTarget robTarget, String input)
        {

            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            short positionIndex = 0;

            string[] words = input.Split(new[] { ' ', ',', '[', ']' });

            foreach (string s in words)
            {
                if (s.Length > 0)
                {
                    try
                    {
                        float value = float.Parse(s, nfi);
                        robTarget.set_Orient(positionIndex, value);
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine(e.ToString());
                        return false;
                    }
                    positionIndex++;
                }
            }

            return true;
        }
    
        ///<summary>
        ///</summary>
        ///<param name="robPosData"></param>
        ///<returns></returns>
        public static String GetRotPosDataPosition(S4RobPosData robPosData)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("[{0},{1},{2}]", robPosData.get_Pos(0).ToString(nfi), robPosData.get_Pos(1).ToString(nfi),robPosData.get_Pos(2).ToString(nfi));
        }

        ///<summary> 
        ///</summary>
        ///<param name="robPosData"></param>
        ///<returns></returns>
        public static String GetRotPosDataOrientation(S4RobPosData robPosData)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("[{0},{1},{2},{3}]", robPosData.get_Orient(0).ToString(nfi), robPosData.get_Orient(1).ToString(nfi),
                                robPosData.get_Orient(2).ToString(nfi), robPosData.get_Orient(3).ToString(nfi));
        }

        public static String GetRotPosDataPositionSimple(S4RobPosData robPosData)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("{0} {1} {2}", robPosData.get_Pos(0).ToString(nfi), robPosData.get_Pos(1).ToString(nfi), robPosData.get_Pos(2).ToString(nfi));
        }

        public static String GetRotPosDataOrientationSimple(S4RobPosData robPosData)
        {
            NumberFormatInfo nfi = new CultureInfo("en-US", false).NumberFormat;

            return String.Format("{0} {1} {2} {3}", robPosData.get_Orient(0).ToString(nfi), robPosData.get_Orient(1).ToString(nfi),
                                robPosData.get_Orient(2).ToString(nfi), robPosData.get_Orient(3).ToString(nfi));
        }

        public static long FillRobTargetVar(S4RobTarget robTarget, string position, string orientation)
        {
            var stopwatch = Stopwatch.StartNew();

            if (!position.Equals(String.Empty))
                ObjectHelper.FillRobTargetPosition(robTarget, position);

            if (!orientation.Equals(String.Empty))
                ObjectHelper.FillRobTargetOrientation(robTarget, orientation);
            
            return stopwatch.ElapsedMilliseconds;
        }
    
    }
}
